#include "HingeBar.h"

HingeBar::HingeBar(void)
{
	length = 10.0;
	width = 1.0;
	color = vector3(1.0, 1.0, 1.0);
}

HingeBar::HingeBar(double l, double w, double m)
{
	length = l;
	width = w;
	color = vector3(1.0, 1.0, 1.0);
	transform.mass = m;
	transform.invMass = 1/m;
	transform.inertia = (m*l*l)/3;
	transform.invInertia = 1/transform.inertia;
}

HingeBar::~HingeBar(void)
{
}

void HingeBar::Update(double t, double dt)
{
	previous = transform;
	Integrate(transform, t, dt);

	double angle = 2 * acos(transform.orientation.w) * 180/pi;
	double deviation = abs(angle - neutral);

	// Constrains motion to the max angle
	if (deviation > maxAngle)
	{
		double nearestAngle;
		if (angle >= neutral)
			nearestAngle = neutral + maxAngle;
		else
			nearestAngle = neutral - maxAngle;

		transform.orientation = quaternion(nearestAngle * pi/180, transform.orientation.getVector().normalized());
		transform.angularMomentum = vector3();
	}
}

void HingeBar::AddTorque(vector3 force, double length)
{
	// Value placeholders
	vector3 r;
	vector3 t;
	double mag;

	// Add torque from a given force and the distance from the hinge
	r = quaternion::rotate(vector3::Right(), transform.orientation)  * length;
	t = r.cross(force).normalized();
	mag = length * force.length() * sin(vector3::angle(r, force));
	transform.torque += t * mag;
}

vector3 HingeBar::GetMomentum(vector3 p)
{
	// Value placeholders
	vector3 r;
	vector3 m;
	double l;
	double mag;

	// Get the displacement vector along the bar
	vector3 toHinge = p - transform.position;
	r = quaternion::rotate(vector3::Right(), transform.orientation);
	l = toHinge.dot(r);

	// Calculate linear momentum vector perpendicular to point
	m = transform.angularMomentum.getVector().cross(r);
	mag = transform.angularMomentum.length()/(length - l);
	return m.normalized() * mag;
}

void HingeBar::Render(void)
{
	// Calculate axis and angle of rotation
	double angle = 2 * acos(transform.orientation.w) * 180/pi;
	vector3 axis = vector3(transform.orientation.x, 
						   transform.orientation.y, 
						   transform.orientation.z);
	axis.normalize();

	glLoadIdentity();
	glTranslated(transform.position.x, transform.position.y, transform.position.z);	
	glRotated(angle, axis.x, axis.y, axis.z);
	glColor3d(color.x, color.y, color.z);
	             
	double hw = width/2;

	glBegin(GL_QUADS);
		glVertex3d(0, hw, 0);
		glVertex3d(length, hw, 0);
		glVertex3d(length, -hw, 0);
		glVertex3d(0, -hw, 0);
	glEnd();
}